/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "imageutils.h"

/**********************************************************************
 *
 * Color correction functions
 *
 **********************************************************************/

/*
 * Turn a YUV4:2:0 block into an RGB block
 *
 * Video4Linux seems to use the blue, green, red channel
 * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
 *
 * Color space conversion coefficients taken from the excellent
 * http://www.inforamp.net/~poynton/ColorFAQ.html
 * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
 * Y values are given for all 4 pixels, but the U (Pb)
 * and V (Pr) are assumed constant over the 2x2 block.
 *
 * To avoid floating point arithmetic, the color conversion
 * coefficients are scaled into 16.16 fixed-point integers.
 * They were determined as follows:
 *
 *	double brightness = 1.0;  (0->black; 1->full scale) 
 *	double saturation = 1.0;  (0->greyscale; 1->full color)
 *	double fixScale = brightness * 256 * 256;
 *	int rvScale = (int)(1.402 * saturation * fixScale);
 *	int guScale = (int)(-0.344136 * saturation * fixScale);
 *	int gvScale = (int)(-0.714136 * saturation * fixScale);
 *	int buScale = (int)(1.772 * saturation * fixScale);
 *	int yScale = (int)(fixScale);	
 */

/* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
#define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))

inline void
move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v, 
	       int rowPixels, unsigned char * rgb, int bits)
{
	const int rvScale = 91881;
	const int guScale = -22553;
	const int gvScale = -46801;
	const int buScale = 116129;
	const int yScale  = 65536;
	int r, g, b;

	g = guScale * u + gvScale * v;
//	if (force_rgb) {
//		r = buScale * u;
//		b = rvScale * v;
//	} else {
		r = rvScale * v;
		b = buScale * u;
//	}

	yTL *= yScale; yTR *= yScale;
	yBL *= yScale; yBR *= yScale;

	if (bits == 24) {
		/* Write out top two pixels */
		rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
		rgb[2] = LIMIT(r+yTL);

		rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
		rgb[5] = LIMIT(r+yTR);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 3 * rowPixels;
		rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
		rgb[2] = LIMIT(r+yBL);

		rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
		rgb[5] = LIMIT(r+yBR);
	} else if (bits == 16) {
		/* Write out top two pixels */
		rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F) 
			| ((LIMIT(g+yTL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07)
			| (LIMIT(r+yTL) & 0xF8);

		rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F) 
			| ((LIMIT(g+yTR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07) 
			| (LIMIT(r+yTR) & 0xF8);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 2 * rowPixels;

		rgb[0] = ((LIMIT(b+yBL) >> 3) & 0x1F)
			| ((LIMIT(g+yBL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yBL) >> 5) & 0x07)
			| (LIMIT(r+yBL) & 0xF8);

		rgb[2] = ((LIMIT(b+yBR) >> 3) & 0x1F)
			| ((LIMIT(g+yBR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yBR) >> 5) & 0x07)
			| (LIMIT(r+yBR) & 0xF8);
	}
}

/* Converts from planar YUV420 to RGB24. */
void 
yuv420p_to_rgb(int width, int height,
	       unsigned char *pIn0, unsigned char *pOut0, int bits)
{
	const int numpix = width * height;
	const int bytes = bits >> 3;
	int i, j, y00, y01, y10, y11, u, v;
	unsigned char *pY = pIn0;
	unsigned char *pU = pY + numpix;
	unsigned char *pV = pU + numpix / 4;
	unsigned char *pOut = pOut0;

	for (j = 0; j <= height - 2; j += 2) {
		for (i = 0; i <= width - 2; i += 2) {
			y00 = *pY;
			y01 = *(pY + 1);
			y10 = *(pY + width);
			y11 = *(pY + width + 1);
			u = (*pU++) - 128;
			v = (*pV++) - 128;

			move_420_block(y00, y01, y10, y11, u, v,
				       width, pOut, bits);
	
			pY += 2;
			pOut += 2 * bytes;

		}
		pY += width;
		pOut += width * bytes;
	}
}

#define YUV2RGB(y, u, v, r, g, b)\
  r = y + ((v * 1434) / 2048);\
  g = y - ((u * 406) / 2048) - ((v * 595) / 2048);\
  b = y + ((u * 2078) / 2048);\
  r = r < 0 ? 0 : r;\
  g = g < 0 ? 0 : g;\
  b = b < 0 ? 0 : b;\
  r = r > 255 ? 255 : r;\
  g = g > 255 ? 255 : g;\
  b = b > 255 ? 255 : b




/*routine to convert an array of YUV data to RGB format
  from Bart Nabbe*/

int yuv422torgb (char *YUV, char *RGB, int NumPixels) {
	int i, j;
	register int y0, y1, u, v;
	register int r, g, b;

	for (i = 0, j = 0; i < 2 * NumPixels; i += 4, j += 6)
	{
		u = (unsigned char) YUV[i + 0] - 128;
		y0 = (unsigned char) YUV[i + 1];
		v = (unsigned char) YUV[i + 2] - 128;
		y1 = (unsigned char) YUV[i + 3];
		YUV2RGB (y0, u, v, r, g, b);
		RGB[j + 0] = r;
		RGB[j + 1] = g;
		RGB[j + 2] = b;
		YUV2RGB (y1, u, v, r, g, b);
		RGB[j + 3] = r;
		RGB[j + 4] = g;
		RGB[j + 5] = b;
	}
	return 0;
}
/**
* Converts the image from YUV422 format to YUV420P 
* In YUV
* out YUV2
* M = width
* N = Height
*/
int yuv422toyuv420p (char *YUV, char *YUV2, int M, int N) {
	int i, j,iM,iM4;
	int y0, y1, u, v;
	char *Y, *U, *V;
	
	Y=YUV2;
	U=YUV2+M*N;
	V=YUV2+M*N+M*N/4;
	for (i = 0; i < N; i+=2)
	{
		iM=i*M;
		iM4=iM/4;
		for (j = 0; j < M; j+=2)
		{
			u = (unsigned char) YUV[(iM+j)*2+0];
			y0 = (unsigned char) YUV[(iM+j)*2 + 1];
			v = (unsigned char) YUV[(iM+j)*2 + 2];
			y1 = (unsigned char) YUV[(iM+j)*2 + 3];
			Y[iM+j+0]=y0;
			Y[iM+j+1]=y1;
			U[iM4+j/2]=u;
			V[iM4+j/2]=v;
		}
		
		for (; j < 2*M; j+=2)
		{
			u = (unsigned char) YUV[(iM+j)*2+0];
			y0 = (unsigned char) YUV[(iM+j)*2 + 1];
			v = (unsigned char) YUV[(iM+j)*2 + 2];
			y1 = (unsigned char) YUV[(iM+j)*2 + 3];
			Y[iM+j+0]=y0;
			Y[iM+j+1]=y1;
		}	
	}
	return 0;
}


/**
* Saves PGM image (gray scale image)
*/
void imageutils_pgm_save(unsigned char* im,char* filename,int width,int height)
{
  FILE* fd;
  int i,j;

  fd=fopen(filename,"wb");
  if(fd==NULL)
    {
      printf("imageTools_pgm_save::error\n");
      exit(-1);
    } 

  fprintf(fd,"P2\n");
  fprintf(fd,"%d %d\n",width,height);
  fprintf(fd,"255\n");

  for(i=0;i<height;i++)
    {
      for(j=0;j<width;j++)
	{
	  fprintf(fd,"%d ",(int)im[i*(width)+j]);
	}
      fprintf(fd,"\n");
    }
  fclose(fd);
}
/**
* PPM save. Saves PPM image (RGB)
*/
void imageutils_ppm_save(unsigned char* im,char* filename,int width,int height)
{
  FILE* fd;
 

  fd=fopen(filename,"wb");
  if(fd==NULL)
    {
      printf("imageTools_ppm_save::error\n");
      exit(-1);
    }
  fprintf(fd,"P6\n");
  fprintf(fd,"%d %d\n",width,height);
  fprintf(fd,"255\n");

  fwrite(im,1,3*width*height,fd);

  fclose(fd);
}
/**
* Load PPM image
*/
void imageutils_ppm_load(unsigned char* im,char* filename,int *width,int *height)
{
  FILE* fd;
 

  fd=fopen(filename,"rb");
  if(fd==NULL)
    {
      printf("imageTools_ppm_load::error\n");
      exit(-1);
    }
    fscanf(fd,"P6\n");
    
  fscanf(fd,"%d %d\n",width,height);
  fscanf(fd,"255\n");

  fread(im,1,3* (*width)*(*height),fd);

  fclose(fd);
}
/**
* Flips RGB image - be careful that you have proper parameters and actually RGB image
* @param img The image data
* @param width The width of the image in pixels (one pixel has R,G and B value)
* @param height The height of the image in pixels
*/
void imageutils_flip_rgbimage(unsigned char *img, unsigned int width, unsigned int height){
    int row,col;
    unsigned char pix_up;
    for(row = 0; row < height/2 ; row ++){
	for(col = 0; col < 3 * width; col++){
	    pix_up = img[3 * width *row + col];
	    img[3 * width*row + col] = img[3 * width * height - 3 * width *row  + col];
	    img[3 * width * height - 3 * width *row  + col] = pix_up;
	}
    }
    
    
}
/**
* Undistort RGB image - be careful that you have proper parameters and actually RGB image
* @param warped The undistorted image data
* @param imA The image data
* @param k1 The undistortion parameter
* @param k2 The undistortion parameter
* @param u0 The undistortion center x-coordinate
* @param v0 The undistortion center y-coordinate
* @param M The width of the image in pixels (one pixel has R,G and B value)
* @param N The height of the image in pixels
*/
void undist_rgb_image(unsigned char *warped, unsigned char *imA, double k1, 
		      double k2,double u0, double v0, int M, int N)
{
	int i,j,xo,yo,index,M3;
	double v,u,ax,ay,x,y,r2,w1,w2,w3,w4;
   
    	M3=M*3;
	for(i=0;i<N;i++)
	{
		for(j=0;j<M;j++)
		{
			x=(i-v0);
			y=(j-u0);
			r2=x*x+y*y;
			u=(k1+(k2*r2))*x;
			v=(k1+(k2*r2))*y;

			if(u<0)
			{
				xo=(int)u-1;
				ax=(int)u-xo;
			}
			else
			{
				xo=(int)u;
				ax=u-xo;
                
			}
			if(v<0)
			{
				yo=(int)v-1;
				ay=v-yo;
                
			}
			else
			{
				yo=(int)v;
				ay=v-yo;
			}
			
			if((i+xo+1>=N)||(i+xo<0)||(j+yo<0)||(j+yo+1>=M))
			{
				warped[i*M3+j*3]=0;
				warped[i*M3+j*3+1]=0;
				warped[i*M3+j*3+2]=0;
			}
			else
			{
				w4=ax*ay;
				w1=1-ax-ay+ax*ay;
				w2=ax-ax*ay;
				w3=ay-ax*ay;
				index=(i+xo)*M3+(j+yo)*3;
				
				warped[i*M3+3*j]=(unsigned char)abs((int)(w1*imA[index]+w2*imA[index+M3]
						+w3*imA[index+3]+w4*imA[index+M3+3]));
				index++;
				warped[i*M3+3*j+1]=(unsigned char)abs((int)(w1*imA[index]+w2*imA[index+M3]
						+w3*imA[index+3]+w4*imA[index+M3+3]));
				index++;
				warped[i*M3+3*j+2]=(unsigned char)abs((int)(w1*imA[index]+w2*imA[index+M3]
						+w3*imA[index+3]+w4*imA[index+M3+3]));
			}
		}
	}
}
